(in-package :cl-fast-ecs)


(declaim (ftype (function (array-length) array-length) new-capacity)
         (inline new-capacity))
(defun new-capacity (current-capacity)
  (the array-length (+ (ash current-capacity -1) current-capacity)))

(declaim (inline binary-search))
(defun binary-search (data value greater &key (start 0) end)
  (declare (optimize (safety 0)))
  (loop :with l :of-type array-index := start
        :with r :of-type array-index := end
        :while (< l r)
        :for i :of-type array-index := (floor (the array-index (+ l r)) 2)
        :for a := (aref data i)
        :do (if (funcall greater a value)
                (setf r i)
                (setf l (the array-index (1+ i))))
        :finally (return r)))

(defmacro values-equal (entity data values)
  `(and ,@(mapcar
           (lambda (d v) `(equalp ,v (aref ,d ,entity)))
           data values)))

(defmacro index-insert (index (&rest data) entity (&rest values) unique)
  (let ((composite-p (> (length data) 1)))
    (once-only (index)
      `(block index-insert
         (let* ((values ,(if composite-p `(list ,@values) (first values)))
                (i (rem (sxhash values) (the positive-fixnum (length ,index))))
                (bucket (aref ,index i))
                (bucket-size (if bucket (aref bucket 0) 0)))
           (declare (dynamic-extent values))
           (if (not bucket)
               ;; NOTE: SBCL fits 6 element simple-array into single cache line
               (let ((initial-size 6))
                 (setf bucket (make-array initial-size :element-type 'fixnum
                                                       :initial-element -1)
                       (aref bucket 0) 0
                       (aref ,index i) bucket))
               (when (>= (1+ bucket-size) (length bucket))
                 (adjust-simple-arrayf bucket (new-capacity bucket-size)
                                       :element-type 'fixnum
                                       :initial-element -1)
                 (setf (aref ,index i) bucket)))
           ,(when unique
              `(loop :for i :of-type array-index :from 1 :to bucket-size
                     :for ent :of-type entity := (aref bucket i)
                     :when (values-equal ent ,data ,values)
                     :do (return-from index-insert ent)))
           (let ((i (binary-search bucket ,entity #'>
                                   :start 1 :end (1+ bucket-size))))
             (replace bucket bucket :start1 i :start2 (1- i))
             (setf (aref bucket 0) (1+ bucket-size)
                   (aref bucket i) ,entity))
           nil)))))

(defmacro index-delete (index entity (&rest values))
  (let ((composite-p (> (length values) 1)))
    (once-only (index)
      `(let* ((values ,(if composite-p `(list ,@values) (first values)))
              (i (rem (sxhash values) (the positive-fixnum (length ,index))))
              (bucket (aref ,index i)))
         (declare (dynamic-extent values))
         (when #-ecs-release bucket #+ecs-release t
           (let* ((bucket-size (aref bucket 0))
                  (i (binary-search bucket ,entity #'>
                                    :start 1 :end (1+ bucket-size))))
             (setf (aref bucket 0) (1- bucket-size))
             (replace bucket bucket :start1 (1- i) :start2 i)))))))

(defmacro index-maybe-grow (index count (&rest data) exists
                            min-entity max-entity)
  (let ((value-names (make-gensym-list (length data))))
    (once-only (count)
      `(when (> ,count (length ,index))
         (let* ((new-size (new-capacity ,count))
                (new-index (make-array new-size
                                       :element-type
                                       '(or null (simple-array fixnum (*)))
                                       :initial-element nil)))
           (declare
            (type (simple-array (or null (simple-array fixnum (*))) (*))
                  new-index))
           (loop :for entity :of-type entity :from ,min-entity :to ,max-entity
                 :unless (zerop (sbit ,exists entity))
                 :do (let ,(mapcar (lambda (n d) `(,n (aref ,d entity)))
                            value-names data)
                       (index-insert new-index ,data entity ,value-names nil))
                 :finally (return new-index)))))))

(defmacro index-bucket (index (&rest values))
  (let ((composite-p (> (length values) 1)))
    (once-only (index)
      `(locally
         (declare (type (simple-array (or null (simple-array fixnum (*))) (*))
                        ,index))
         (if (length= 0 ,index)
             nil
             (let ((values ,(if composite-p `(list ,@values) (first values))))
               (declare (dynamic-extent values))
               (aref ,index (rem (sxhash values)
                                 (the positive-fixnum (length ,index))))))))))

(defmacro index-lookup-1 (index (&rest data) (&rest values))
  `(let ((bucket (index-bucket ,index ,values)))
     (if bucket
         (loop :for i :of-type array-index :from 1 :to (aref bucket 0)
               :for entity :of-type entity := (aref bucket i)
               :when (values-equal entity ,data ,values) :return entity)
         nil)))

(defmacro index-lookup (index (&rest data) (&rest values)
                        &optional (count (- array-dimension-limit 2))
                                  (start 0))
  `(let ((bucket (index-bucket ,index ,values)))
     (if bucket
         (loop :with result := nil
               :with c :of-type array-index := 0
               :with s :of-type array-index := 0
               :for i :of-type array-index :from 1 :to (aref bucket 0)
               :for entity :of-type entity := (aref bucket i)
               :when (= c ,count) :do (loop-finish)
               :when (values-equal entity ,data ,values)
               :do (when (>= s ,start)
                     (push entity result)
                     (incf c))
                   (incf s)
               :finally (return (nreverse result)))
         nil)))

(defmacro index-enumerate (index (&rest data) (&rest values) entity &body body)
  (with-gensyms (bucket i)
    `(let ((,bucket (index-bucket ,index ,values)))
       (if ,bucket
           (loop :for ,i :of-type array-index :from 1 :to (aref ,bucket 0)
                 :for ,entity :of-type array-index := (aref ,bucket ,i)
                 :when (values-equal ,entity ,data ,values)
                 :do ,@body)
           nil))))
